#include "stdafx.h"
#include "IntSog.h"
#include "DataSignal.h"
#include <vector>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
using namespace std;

IntSog::IntSog(){}

IntSog::~IntSog()
{

}

void IntSog::Calc_onefile_unbias_intsogs(const string &filename,double &actor_label,double &label,IntsogsType &temp)
{
	DataSignal *sr = new DataSignal;
	vector<vector<double>> signal;
	sr->Get_5points_Signal_onefile(filename,actor_label,label,signal);
	delete sr;
	IntsogsType feas_bias;
	Calc_onefile_bias_intsogs(signal,feas_bias);
	CvMat* biases = Get_biases();

	int nchannel = (int)signal[0].size();
	int nframe = (int)signal.size();
	for(int i = 0 ; i != nframe; i++)
		for(int j = 0 ; j != nchannel; j++)
		{
			signal[i][j] -= ((double*)(biases->data.ptr + ((int)actor_label-1) * biases->step))[j];
		}

	cout<<"-------开始计算unbias_intsog特征------"<<endl;
	CvMat* signal_mat = Vecsignal2Mat(signal);
	CvMat* temp_P = cvCreateMat((signal_mat->rows + 1),signal_mat->cols,CV_64FC1);
	cvSetZero(temp_P);
	Calc_P(signal_mat,temp_P);
	temp.P = temp_P;
	vector<CvMat*> temp_Q;
	Calc_Q(signal_mat,temp_Q);
	temp.Q = temp_Q;
	cvReleaseMat(&signal_mat);

}

void IntSog::Calc_allfile_unbias_intsogs(const std::string &path, CvMat *&actor_labels, CvMat *&labels, std::vector<IntsogsType> &feas_unbias)
{
	DataSignal *sr = new DataSignal;
	vector<double> actor_labels_vec,labels_vec;
	vector<vector<vector<double>>> signals;
	sr->Get_5points_Signals(path,actor_labels_vec,labels_vec,signals);
	delete sr;

	actor_labels = cvCreateMat((int)actor_labels_vec.size(),1,CV_64FC1);
	labels = cvCreateMat((int)labels_vec.size(),1,CV_64FC1);
	for(int i = 0; i != actor_labels_vec.size();i++)
	{
		((double*)(actor_labels->data.ptr + i * actor_labels->step))[0] = actor_labels_vec[i];
		((double*)(labels->data.ptr + i * labels->step))[0] = labels_vec[i];
	}

	vector<IntsogsType> feas_bias;
	Calc_allfile_bias_intsogs(signals,feas_bias);
	CvMat* biases = Calc_biases(feas_bias,actor_labels);
	for(int i = 0; i != feas_bias.size(); i++)
	{
		cvReleaseMat(&feas_bias[i].P);
		for(int j = 0; j != feas_bias[i].Q.size(); j++)
			cvReleaseMat(&feas_bias[i].Q[j]);
		feas_bias[i].Q.clear();
	}
	feas_bias.clear();

	int nchannel = (int)signals[0][0].size();
	for(int m = 0 ; m != signals.size() ; m++)
	{
		int actor_label = (int)(((double*)(actor_labels->data.ptr + m * actor_labels->step))[0]);
		int nframe = (int)signals[m].size();
		for(int i = 0 ; i != nframe; i++)
			for(int j = 0 ; j != nchannel; j++)
			{
				signals[m][i][j] -= ((double*)(biases->data.ptr + (actor_label-1) * biases->step))[j];
			}
	}
	cvReleaseMat(&biases);

	cout<<"-------开始计算unbias_intsog特征------"<<endl;
	for(int i = 0; i != signals.size() ; i++)
	{
		cout<<"---- "<< i+1<<" ----"<<endl;
		CvMat* signal = Vecsignal2Mat(signals[i]);
		IntsogsType temp;
		CvMat* temp_P = cvCreateMat((signal->rows + 1),signal->cols,CV_64FC1);
		cvSetZero(temp_P);
		Calc_P(signal,temp_P);
		temp.P = temp_P;
		vector<CvMat*> temp_Q;
		Calc_Q(signal,temp_Q);
		temp.Q = temp_Q;
		feas_unbias.push_back(temp);
		cvReleaseMat(&signal);
	}
}

//从文件中直接读取biases矩阵
CvMat* IntSog::Get_biases()
{
	string filename = "biases.txt";
	//cout<<"-----读取biases.txt文件数据" <<endl;
	ifstream infile(filename.c_str());
	vector<vector<double>> da;
	int count = 0;
	if(!infile.fail())
	{
		vector<double> xx;
		while(!infile.eof())
		{
			double temp = 0;
			//vector<double> xx;
			infile>>temp;
			count++;
			xx.push_back(temp);
			if(count%21 == 0)
			{	
				da.push_back(xx);
				xx.clear();
			}
		}	
	}
	infile.close();
	CvMat* result = cvCreateMat((int)da.size(),21,CV_64FC1);
	for(int i = 0 ; i  !=  result->rows; i++)
		for(int j = 0; j != result->cols; j++)
		{
			((double*)(result->data.ptr + i * result->step))[j] = da[i][j];
		}
		return result;
}

//计算biases矩阵(nactors * nchannel)
CvMat* IntSog::Calc_biases(const vector<IntsogsType> &intsogs,const CvMat* actor_labels)
{
	vector<int> enum_actor_labels;
	vector<int> ncounts;
	for(int i = 0 ; i !=  actor_labels->rows; i++)
	{
		int temp1 = (int)(((double*)(actor_labels->data.ptr + i * actor_labels->step))[0]);
		enum_actor_labels.push_back(temp1);
		int temp2 = intsogs[i].P->rows - 1;
		ncounts.push_back(temp2);
	}
	sort(enum_actor_labels.begin(),enum_actor_labels.end());
	enum_actor_labels.erase(unique(enum_actor_labels.begin(),enum_actor_labels.end()),enum_actor_labels.end());
	//int nactors = (int)enum_actor_labels.size();
	int nactors = enum_actor_labels[enum_actor_labels.size() -1];
	int nchannel = intsogs[0].P->cols;
	CvMat* sum_signals = cvCreateMat(nactors,nchannel,CV_64FC1);
	cvSetZero(sum_signals);
	CvMat* sum_nframes = cvCreateMat(nactors,1,CV_64FC1);
	cvSetZero(sum_nframes);

	for(int m = 0; m != intsogs.size(); m++)
	{
		int actor_label =(int)((double*)(actor_labels->data.ptr + m * actor_labels->step))[0];
		for(int j = 0; j != nchannel; j++)
		{
			((double*)(sum_signals->data.ptr + (actor_label-1) * sum_signals->step))[j] += 
				((double*)(intsogs[m].P->data.ptr + (intsogs[m].P->rows - 1) * intsogs[m].P->step))[j];	
		}
		((double*)(sum_nframes->data.ptr + (actor_label-1) * sum_nframes->step))[0] += ncounts[m];
	}

	CvMat* biases = cvCreateMat(nactors,nchannel,CV_64FC1);
	cvSetZero(biases);
	for(int i = 0 ; i !=  nactors ; i++)
		for(int j = 0 ; j != nchannel ; j++)
		{
			((double*)(biases->data.ptr + i * biases->step))[j] = 
				((double*)(sum_signals->data.ptr + i * sum_signals->step))[j]/((double*)(sum_nframes->data.ptr + i * sum_nframes->step))[0];
		}

	cvReleaseMat(&sum_signals);
	cvReleaseMat(&sum_nframes);
	return biases;
}

//计算有偏差的intsog特征
void IntSog::Calc_allfile_bias_intsogs(const std::vector<vector<vector<double> > > &signals, vector<IntsogsType> &intsog_feas)
{
	for(int i = 0; i != signals.size(); i++)
	{
		CvMat* signal = Vecsignal2Mat(signals[i]);
		IntsogsType temp;
		CvMat* temp_P = cvCreateMat(signal->rows+1,signal->cols,CV_64FC1);
		cvSetZero(temp_P);
		Calc_P(signal,temp_P);
		temp.P = temp_P;
		vector<CvMat*> temp_Q;
		Calc_Q(signal,temp_Q);
		temp.Q = temp_Q;
		intsog_feas.push_back(temp);
		cvReleaseMat(&signal);
	}
}

void IntSog::Calc_onefile_bias_intsogs(const std::vector<vector<double> > &signals, IntsogsType &temp)
{
	CvMat* signal = Vecsignal2Mat(signals);
	CvMat* temp_P = cvCreateMat(signal->rows+1,signal->cols,CV_64FC1);
	cvSetZero(temp_P);
	Calc_P(signal,temp_P);
	temp.P = temp_P;
	vector<CvMat*> temp_Q;
	Calc_Q(signal,temp_Q);
	temp.Q = temp_Q;
	cvReleaseMat(&signal);
}


void IntSog::Calc_P(const CvMat* signal, CvMat* intSignal)
{
	assert((signal->rows + 1) == intSignal->rows && signal->cols ==intSignal->cols);

	double *temp = new double[intSignal->cols];
	for(int i = 0 ; i != intSignal->cols ; i++)
		temp[i] = 0;	
	cvSetZero(intSignal);
	for(int i = 0 ; i != intSignal->rows - 1; i++)
	{
		for(int j = 0 ; j != intSignal->cols ; j++)
		{
			temp[j] += ((double*)(signal->data.ptr + i * signal->step))[j];
			((double*)(intSignal->data.ptr + (i+1) * intSignal->step))[j] = temp[j];
		}
	}
	delete[] temp;
}

void IntSog::Calc_Q(const CvMat* signal, vector<CvMat*> &Q)
{
	int channels = (int)signal->cols;
	int numframes = (int)signal->rows;
	for(int i = 0; i != channels * channels; i++)
		Q.push_back(NULL);
	for(int n = 0 ; n != channels ; n++)
		for(int k = 0; k != (n + 1); k++)
		{
			CvMat* temp1 = cvCreateMat(signal->rows,1,CV_64FC1);
			CvMat* temp2 = cvCreateMat(signal->rows,1,CV_64FC1);
			CvMat* dst = cvCreateMat(signal->rows,1,CV_64FC1);
			for(int p = 0; p != signal->rows ; p++)
			{
				((double*)(temp1->data.ptr + p * temp1->step))[0] =  ((double*)(signal->data.ptr + p * signal->step))[n];
				((double*)(temp2->data.ptr + p * temp2->step))[0] =  ((double*)(signal->data.ptr + p * signal->step))[k];	
			}
			cvMul(temp1,temp2,dst);
			cvReleaseMat(&temp1);
			cvReleaseMat(&temp2);

			CvMat* tp_Q = cvCreateMat(signal->rows+1,1,CV_64FC1);
			cvSetZero(tp_Q);
			Calc_P(dst,tp_Q);
			cvReleaseMat(&dst);
			Q[n * channels + k] = tp_Q;
			if(n != k)
			{
				CvMat* tp_Q2 = (CvMat*)cvClone(tp_Q);
				Q[k* channels + n] = tp_Q2;
				//Q[k* channels + n] = Q[n * channels + k];		
			}
		}
}

CvMat* IntSog::Vecsignal2Mat(const std::vector<vector<double>> &signal)
{
	int rows = (int)signal.size();
	int cols = (int)signal[0].size();
	CvMat *mat = cvCreateMat(rows,cols,CV_64FC1);
	for(int i = 0; i != rows; i++)
		for(int j = 0; j != cols; j++)
			((double*)(mat->data.ptr + i * mat->step))[j] = signal[i][j];
	return mat;
}